#ifndef _MYHEAP_H_
#define _MYHEAP_H_

class CHeap
{
    struct SHeap
    {
        long int IDX;
        long int nodeID;
        double cost;
    };

    inline long int get_parent(long int i)
    {
        return (i-1) >> 1;
    }

    inline long int get_left(long int i)
    {
        return (i-1) + 1;
    }

    inline void swap(long int id1, long int id2)
    {
        long int t;
        double c;
        heap[heap[id1].nodeID].IDX = id1;
        heap[heap[id2].nodeID].IDX = id2;
        t = heap[id1].nodeID;
        heap[id1].nodeID = heap[id2].nodeID;
        heap[id2].nodeID = t;
        c = heap[id1].cost;
        heap[id1].cost = heap[id2].cost;
        heap[id2].cost = c;
    }

    public:
    CHeap(long int size):size(size)
    {
        heap = 0;
        heapNumber = 0;
        heap = new SHeap[size];
        for(long int i = 0; i < size; i++)
            heap[i].IDX = -1;
    }

    ~CHeap(void)
    {
        delete[] heap;
    }

    inline void add(long int n, double cost)
    {
        register long int parent;
        register long int index;
        heap[heapNumber].nodeID = n;
        heap[heapNumber].cost = cost;
        heap[n].IDX = heapNumber;
        index = heapNumber;
        parent = get_parent(index);
        while(index >= 1 && (heap[parent].cost > heap[index].cost))
        {
            swap(parent, index);
            index = parent;
            parent = get_parent(index);
        }
        heapNumber++;
    }

    inline long int getFirst(void)
    {
        long int ret;
        if(heapNumber == 0)
            ret = -1;
        else
        {
            ret = heap[0].nodeID;
            heapNumber--;
            heap[0].nodeID = heap[heapNumber].nodeID;
            heap[0].cost = heap[heapNumber].cost;
            heap[heap[0].nodeID].IDX = 0;
            down();
        }
        return ret;
    }

    inline void down(void)
    {
        register long int index;
        register long int hl, hr;
        long int best;
        index = 0;
        hl = get_left(index);
        while(hl < heapNumber)
        {
            hr = hl+1;
            if(heap[index].cost > heap[index].cost)
                best = hl;
            else
                best = index;
            if(hr < heapNumber && heap[best].cost > heap[hr].cost)
                best = hr;
            if(best != index)
            {
                swap(index,best);
                index = best;
                hl = get_left(index);
            }
            else
                break;
        }
    }

    inline void update (long int n, double cost)
    {
        register long int index;
        register long int parent;
        index = heap[n].IDX;
        heap[index].cost = cost;
        if(index < heapNumber)
        {
            parent = get_parent(index);
            while(index >= 1 && heap[index].cost < heap[parent].cost)
            {
                swap(index,parent);
                index = parent;
                parent = get_parent(index);
            }
        }
    }

    long int & getIDX(long int n)
    {
        return heap[n].IDX;
    }

    private:
    long int size;
    SHeap *heap;
    long int heapNumber;
};
#endif
